#ifndef CAR_H
#define CAR_H

#include <math.h>
#include <QDebug>
#include <QPointF>
#include <vector>
#include <iostream>
using namespace std;
#define MAX_ACC 50

#define SPEED_STEP 0.5
#define ANGLE_STEP 0.4

#define PI 3.141592653
#define NUM 10

#define ANGLE_SPEED 0.3
#define ANGLESPEED_STEP 0.15

#define CAR_W 10
#define CAR_H 15

#define MAX_SPEED 10
#define FACC 0.1

#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))

typedef struct{
    int x,y;
    double angle;
}IPoint;


class Car
{
public:
    Car(){//int mX, int mY
        initx=100;
        inity=100;
        initAngle=PI;
        init(FACC,MAX_SPEED);
        sita=atan(1.0*CAR_W/CAR_H);
        width=sqrt(CAR_W*CAR_W+CAR_H*CAR_H);
    }

    void initMap(int w, int h)
    {
        map=(char**)malloc(sizeof(char*)*(w+1));
        for(int i=0;i<w+1;i++)
        {
            map[i]=(char*)malloc(sizeof(char)*(h+1));
        }
    }

    void init(double facc,double mspeed)
    {
        vx=0;
        vy=0;
        speed=0;
        acc=0;
        angle=initAngle;
        x=initx;
        y=inity;
        max_x=2000;
        max_y=2000;
        dst_angle=0;
        call_times=0;
        step=1.0/NUM;
        angle_speed=0;
        max_speed=mspeed;
        end=0;
        f=facc;
        path.clear();
    }
    
    void setAngleSpeed(double value){
        angle_speed = value;
        if(angle_speed>ANGLE_SPEED)
            angle_speed=ANGLE_SPEED;
        else if(angle_speed<-ANGLE_SPEED)
            angle_speed=-ANGLE_SPEED;
    }

    void setAngle(double value){
        angle+=value;
        if(angle>2*PI)
            angle-=2*PI;
        else if(angle<-2*PI)
            angle+=2*PI;
    }

    void setAcc(double value){
        acc=value;
        if(acc>MAX_ACC)
            acc=MAX_ACC;
        else if(acc<-MAX_ACC)
            acc=-MAX_ACC;
    }

    void setSpeed(double value){
        speed+=value;
        if(speed>max_speed)
            speed=max_speed;
        else if(speed<0)
            speed=0;
    }

    void cal_fv()
    {
        if(vx>0)
            vx=MAX(vx-f*step,0);
        else if(vx<0)
            vx=MIN(0,vx+f*step);

        if(vy>0)
            vy=MAX(vy-f*step,0);
        else if(vx<0)
            vy=MIN(0,vy+f*step);
    }

    void update_speed()
    {
        vy+=-1*acc*cos(angle)*step;
        vx+=acc*sin(angle)*step;
        double ang=1.57;
        if(vy!=0)
            ang=atan(fabs(vx/vy));
        double max_sy=fabs(max_speed*cos(ang));
        double max_sx=fabs(max_speed*sin(ang));

        if(vy>=max_sy)
            vy=max_sy;
        else if(vy<-max_sy)
            vy=-max_sy;

        if(vx>=max_sx)
            vx=max_sx;
        else if(vx<-max_sx)
            vx=-max_sx;
    }

    void checkEnd(int sx, int sy, int ex, int ey)
    {
        int t=0;
        if(ex<sx)
            t=sx,sx=ex,ex=t;
        if(ey<sy)
            t=sy,sy=ey,ey=t;

        for(int i=sx;i<=ex;i++){
            for(int j=sy;j<=ey;j++){
                if(map[i][j]==2)
                {
                    end++;
                    return;
                }
            }
        }
    }

    void update()
    {
        setAngle(angle_speed*step);
        update_speed();
        cal_fv();
        double tx=x+vx*step;
        double ty=y+vy*step;
        checkEnd(x,y,tx,ty);
        if(!checkCrash(tx,ty))
        {
            x+=vx*step;
            y+=vy*step;
        }
        else if(!checkCrash(tx,y))
        {
            x+=vx*step;
            vy=0;
        }
        else if(!checkCrash(x,ty))
        {
            y+=vy*step;
            vx=0;
        }
        else
        {
            vx=0;
            vy=0;
        }

        if(x>max_x)
            x=max_x;
        else if(x<0)
            x=0;

        if(y>max_y)
            y=max_y;
        else if(y<0)
            y=0;

        IPoint t;
        t.x=(int)x;
        t.y=(int)y;
        t.angle=angle;
        path.push_back(t);
    }

    int getX()
    {
        return (int)x;
    }
    int getY()
    {
        return (int)y;
    }
    double getAngle()
    {
        return angle;
    }

    double vx,vy,speed,angle;
    double acc;
    double x,y;
    double dst_angle;
    int max_x,max_y;
    int call_times;
    double step;
    double angle_speed;
    double sita;
    double width;
    double f;
    double max_speed;
    int end;
    int initx,inity;
    double initAngle;
    char **map;

    QPointF points[5];
    vector<IPoint> path;

    void calPoints(double x, double y)
    {
        double x1,y1,x2,y2;
        x1=width*sin(angle-sita)+x;
        y1=-1*width*cos(angle-sita)+y;

        x2=width*sin(angle+sita)+x;
        y2=-1*width*cos(angle+sita)+y;
        points[0].setX((x1+x2)*0.5);
        points[0].setY((y1+y2)*0.5);

//        points[3].setX((x-x1)+x);
//        points[3].setY((y-y1)+y);

//        points[2].setX((x-x2)+x);
//        points[2].setY((y-y2)+y);

//        points[1].setX((x1+points[2].x())*0.5);
//        points[1].setY((y1+points[2].y())*0.5);

//        points[4].setX((x2+points[3].x())*0.5);
//        points[4].setY((y2+points[3].y())*0.5);
    }

    bool checkCrash(double x, double y)
    {
        if(map[(int)x][(int)y]==1)
            return true;
        else
            return false;
        calPoints(x,y);
        for(int i=0;i<1;i++)
        {
            if(map[(int)points[i].x()][(int)points[i].y()]==1)
                return true;
        }
        return false;
    }


};

#endif // CAR_H
